/* Services.cc -- Serviços oferecidos ao resto do mundo
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 *
 * This file is part of GEARMedula.
 * 
 * GEARMedula is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, version 3 of the
 * License.
 * 
 * GEARMedula is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see
 * <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <vector>

#include "Services.hh"
#include "Control.hh"
#include "Monitor.hh"
#include "XError.hh"
#include "Messages.hh"

void
Services::setMonitor( Monitor * monitor )
{
    monitor_ = monitor;
}

void
Services::setControl( Control * control )
{
    control_ = control;
}

int
Services::ExecServiceOfMessage( MessageType message_type,
                                const std::vector<struct FieldContainer> args_passed,
                                std::vector<struct FieldContainer> &args_returned )
{
    switch( message_type )
    {
    case MESSAGETYPE_SET_WORLD:
    {
        std::vector<float> world_vec(kWorldVectorLength);
        for( unsigned int i = 0 ; i < world_vec.size() ; i++ )
        {
            world_vec[i] = args_passed[0].field_world_vector[i];
        }            

        monitor_->setWorld( world_vec );
        
        break;
    }
    case MESSAGETYPE_SET_FIELD:
    {
        std::vector<float> field_vec(kFieldVectorLength);
        for( unsigned int i = 0 ; i < field_vec.size() ; i++ )
        {
            field_vec[i] = args_passed[0].field_field_vector[i];
        }            

        monitor_->setField( field_vec );
        
        break;
    }
    case MESSAGETYPE_GET_WORLD:
    {
        args_returned.resize(1);
        std::vector<float> world_vec(kWorldVectorLength);
        monitor_->getWorld( world_vec );
        for( unsigned int i = 0 ; i < world_vec.size() ; i++ )
        {
            args_returned[0].field_world_vector[i] = world_vec[i];
        }            

        return 1;
        break;
    }
    case MESSAGETYPE_GET_FIELD:
    {
        args_returned.resize(1);
        std::vector<float> field_vec(kFieldVectorLength);
        monitor_->getField( field_vec );
        for( unsigned int i = 0 ; i < field_vec.size() ; i++ )
        {
            args_returned[0].field_field_vector[i] = field_vec[i];
        }            

        return 1;
        break;
    }
    case MESSAGETYPE_SAY_HELLO:
    {
        break;
    }
    case MESSAGETYPE_SET_VEL:
    {

        int robot_id = args_passed[0].field_integer;
        float vx = args_passed[1].field_float;
        float vy = args_passed[2].field_float;
        float va = args_passed[3].field_float;

        control_->robotSetVel( robot_id, vx, vy, va );

        break;
    }
    case MESSAGETYPE_KICK:
    {
        int robot_id = args_passed[0].field_integer;
        int nivel = args_passed[1].field_integer;

        control_->robotKick( robot_id, nivel );

        break;
    }
    case MESSAGETYPE_DRIBBLE_ENABLE:
    {
        int robot_id = args_passed[0].field_integer;
        int enable = args_passed[1].field_integer;

        control_->robotDribbleEnable( robot_id, enable );

        break;
    }
    case MESSAGETYPE_REGISTER_ROBOT_NAME:
    {
        int robot_id = args_passed[0].field_integer;
        std::string robot_name(args_passed[1].field_string);

        control_->RegisterRobotName( robot_id, robot_name );

        break;
    }
    default:
    {
        xerror_.Error("Não conheço este serviço requisitado.");
        break;
    }
    }

    return 0;
}
